// Include files.
#include <cmath>
#include "../Internal/FpkAssert.hpp"
#include "../Internal/FpkException.hpp"
#include "../Internal/NullDeleter.hpp"
#include "../Actuators/CanJaguarWrapper.hpp"
#include "TankDriveSubsystem.hpp"
#include "../Filters/Drive/ArcadeDriveFilter.hpp"
#include "../Filters/Drive/TankDriveFilter.hpp"

using namespace std;
using namespace boost;

TankDriveSubsystem::MotorPosition::MotorPosition(TankDriveSubsystem::Side side, UINT32 rank) :
	m_side(side),
	m_rank(rank)
{
}

bool TankDriveSubsystem::MotorPosition::operator<(TankDriveSubsystem::MotorPosition other) const
{
	if (m_side == Left && other.m_side == Right)
		return true;
	else if (m_side != other.m_side)
		return false;
	else
		return m_rank < other.m_rank;
}

RobotDrive::MotorType TankDriveSubsystem::MotorPosition::ToWpiValue(bool sideOnly)
{
	if (sideOnly)
	{
		if (m_side == Left)
			return RobotDrive::kRearLeftMotor;
		else if (m_side == Right)
			return RobotDrive::kRearRightMotor;
		else
			throw FpkException("Invalid side.");
	}
	else
	{
		if (m_side == Left)
		{
			if (m_rank == 0)
				return RobotDrive::kFrontLeftMotor;
			else if (m_rank == 1)
				return RobotDrive::kRearLeftMotor;
			else
				throw FpkException("Motor position rank not supported by WPILib's RobotDrive.");
		}
		else if (m_side == Right)
		{
			if (m_rank == 0)
				return RobotDrive::kFrontRightMotor;
			else if (m_rank == 1)
				return RobotDrive::kRearRightMotor;
			else
				throw FpkException("Motor position rank not supported by WPILib's RobotDrive.");
		}
		else
			throw FpkException("Invalid motor positon side.");
	}
}

/**
 *  Constructs a TankDriveSubsystem with four motors.
 */

TankValue TankDriveSubsystem::TankDriveFetchValue
(
	GenericHID &leftJoystick, 
	GenericHID &rightJoystick
)
{
	return TankDriveFilter().Apply(TwoJoystickValue(leftJoystick, rightJoystick));
}

TankValue TankDriveSubsystem::ArcadeDriveFetchValue(GenericHID &joystick)
{
	return ArcadeDriveFilter().Apply(JoystickValue(joystick));
}

TankDriveSubsystem::ScaleModifier::ScaleModifier(double scale) :
	m_scale(scale)
{
}

void TankDriveSubsystem::ScaleModifier::Modify
(
	TankValue &value
)
{
	value=TankValue(value.GetLeftValue() *m_scale, value.GetRightValue() *m_scale);
}

TankDriveSubsystem::DeadZoneModifier::DeadZoneModifier(double minimumAbsValue) :
	m_minimumAbsValue(minimumAbsValue)
{
}

void TankDriveSubsystem::DeadZoneModifier::Modify
(
	TankValue &value
)
{
	double left = value.GetLeftValue();
	double right = value.GetRightValue();
	
	// LeftValue Deadzone
	if (fabs(left) < m_minimumAbsValue)
		left = 0;
	else if(left >= m_minimumAbsValue)
		left = (1/(1 - m_minimumAbsValue)) * (left - m_minimumAbsValue);
	else
		left = (1/(1 - m_minimumAbsValue)) * (left + m_minimumAbsValue);
	
	// RightValue Deadzone
	if (fabs(right) < m_minimumAbsValue)
		right = 0;
	else if(right >= m_minimumAbsValue)
		right = (1/(1 - m_minimumAbsValue)) * (right - m_minimumAbsValue); 
	else
		right = (1/(1 - m_minimumAbsValue)) * (right + m_minimumAbsValue);
	
	value = TankValue(left,right);
}

void TankDriveSubsystem::ReverseModifier::Modify(TankValue &value)
{
	value = TankValue(-value.GetRightValue(), -value.GetLeftValue());
}

void TankDriveSubsystem::DriveByTank(TankDriveSubsystem &drive, const TankValue &value)
{
	drive.DriveByTank(value.GetLeftValue(), value.GetRightValue());
}

TankDriveSubsystem::TankDriveSubsystem
(
	UINT32 leftFrontMotorChannel, 
	UINT32 leftBackMotorChannel,
	UINT32 rightFrontMotorChannel, 
	UINT32 rightBackMotorChannel
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(false)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotorChannel, 
			leftBackMotorChannel, 
			rightFrontMotorChannel, 
			rightBackMotorChannel
		)
	)
	, m_motorRankCount(2)
{
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two motors.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	UINT32 leftMotorChannel,
	UINT32 rightMotorChannel
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(false)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotorChannel, 
			rightMotorChannel
		)
	)
	, m_motorRankCount(1)
{
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four PidEncoderMotorControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	boost::shared_ptr<PidMotorController> leftFrontMotor, 
	boost::shared_ptr<PidMotorController> leftBackMotor,
	boost::shared_ptr<PidMotorController> rightFrontMotor, 
	boost::shared_ptr<PidMotorController> rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor.get(), 
			leftBackMotor.get(), 
			rightFrontMotor.get(), 
			rightBackMotor.get()
		)
	)
	, m_motorRankCount(2)
{
	SetPidMotorController(Left, 0, leftFrontMotor);
	SetPidMotorController(Left, 1, leftBackMotor);
	SetPidMotorController(Right, 0, rightFrontMotor);
	SetPidMotorController(Right, 1, rightBackMotor);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two PidEncoderMotorControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	boost::shared_ptr<PidMotorController> leftMotor,
	boost::shared_ptr<PidMotorController> rightMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor.get(), 
			rightMotor.get()
		)
	)
	, m_motorRankCount(1)
{
	SetPidMotorController(Left, 0, leftMotor);
	SetPidMotorController(Right, 0, rightMotor);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four PidEncoderMotorControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	PidMotorController *leftFrontMotor, 
	PidMotorController *leftBackMotor,
	PidMotorController *rightFrontMotor, 
	PidMotorController *rightBackMotor,
	bool canDeleteMotors
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
	, m_motorRankCount(2)
{
	SetPidMotorController(Left, 0, leftFrontMotor, canDeleteMotors);
	SetPidMotorController(Left, 1, leftBackMotor, canDeleteMotors);
	SetPidMotorController(Right, 0, rightFrontMotor, canDeleteMotors);
	SetPidMotorController(Right, 1, rightBackMotor, canDeleteMotors);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two PidEncoderMotorControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	PidMotorController *leftMotor,
	PidMotorController *rightMotor,
	bool canDeleteMotors
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor, 
			rightMotor
		)
	)
	, m_motorRankCount(1)
{
	SetPidMotorController(Left, 0, leftMotor, canDeleteMotors);
	SetPidMotorController(Right, 0, rightMotor, canDeleteMotors);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four PidEncoderMotorControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	PidMotorController &leftFrontMotor, 
	PidMotorController &leftBackMotor,
	PidMotorController &rightFrontMotor, 
	PidMotorController &rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
	, m_motorRankCount(2)
{
	SetPidMotorController(Left, 0, leftFrontMotor);
	SetPidMotorController(Left, 1, leftBackMotor);
	SetPidMotorController(Right, 0, rightFrontMotor);
	SetPidMotorController(Right, 1, rightBackMotor);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two PidEncoderMotorControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	PidMotorController &leftMotor,
	PidMotorController &rightMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor, 
			rightMotor
		)
	)
	, m_motorRankCount(1)
{
	SetPidMotorController(Left, 0, leftMotor);
	SetPidMotorController(Right, 0, rightMotor);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four CANJaguars.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	boost::shared_ptr<CANJaguar> leftFrontMotor, 
	boost::shared_ptr<CANJaguar> leftBackMotor,
	boost::shared_ptr<CANJaguar> rightFrontMotor, 
	boost::shared_ptr<CANJaguar> rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor.get(), 
			leftBackMotor.get(), 
			rightFrontMotor.get(), 
			rightBackMotor.get()
		)
	)
	, m_motorRankCount(2)
{
	SetPidMotorController(Left, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(leftFrontMotor)));
	SetPidMotorController(Left, 1, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(leftBackMotor)));
	SetPidMotorController(Right, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(rightFrontMotor)));
	SetPidMotorController(Right, 1, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(rightBackMotor)));
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two CANJaguars.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	boost::shared_ptr<CANJaguar> leftMotor,
	boost::shared_ptr<CANJaguar> rightMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor.get(), 
			rightMotor.get()
		)
	)
	, m_motorRankCount(1)
{
	SetPidMotorController(Left, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(leftMotor)));
	SetPidMotorController(Right, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(rightMotor)));
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four CANJaguars.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	CANJaguar *leftFrontMotor, 
	CANJaguar *leftBackMotor,
	CANJaguar *rightFrontMotor, 
	CANJaguar *rightBackMotor,
	bool canDeleteMotors
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
	, m_motorRankCount(2)
{
	SetPidMotorController(Left, 0, leftFrontMotor, canDeleteMotors);
	SetPidMotorController(Left, 1, leftBackMotor, canDeleteMotors);
	SetPidMotorController(Right, 0, rightFrontMotor, canDeleteMotors);
	SetPidMotorController(Right, 1, rightBackMotor, canDeleteMotors);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two CANJaguars.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	CANJaguar *leftMotor,
	CANJaguar *rightMotor,
	bool canDeleteMotors
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor, 
			rightMotor
		)
	)
	, m_motorRankCount(1)
{
	SetPidMotorController(Left, 0, leftMotor, canDeleteMotors);
	SetPidMotorController(Right, 0, rightMotor, canDeleteMotors);
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four CANJaguars.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	CANJaguar &leftFrontMotor, 
	CANJaguar &leftBackMotor,
	CANJaguar &rightFrontMotor, 
	CANJaguar &rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
	, m_motorRankCount(2)
{
	SetPidMotorController(Left, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(leftFrontMotor)));
	SetPidMotorController(Left, 1, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(leftBackMotor)));
	SetPidMotorController(Right, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(rightFrontMotor)));
	SetPidMotorController(Right, 1, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(rightBackMotor)));
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two CANJaguars.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	CANJaguar &leftMotor,
	CANJaguar &rightMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(true)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor, 
			rightMotor
		)
	)
	, m_motorRankCount(1)
{
	SetPidMotorController(Left, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(leftMotor)));
	SetPidMotorController(Right, 0, shared_ptr<CanJaguarWrapper>(new CanJaguarWrapper(rightMotor)));
	
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four SpeedControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	SpeedController *leftFrontMotor, 
	SpeedController *leftBackMotor,
	SpeedController *rightFrontMotor, 
	SpeedController *rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(false)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
	, m_motorRankCount(2)
{
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two SpeedControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	SpeedController *leftMotor,
	SpeedController *rightMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(false)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor,
			rightMotor
		)
	)
	, m_motorRankCount(1)
{
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with four SpeedControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	SpeedController &leftFrontMotor, 
	SpeedController &leftBackMotor,
	SpeedController &rightFrontMotor, 
	SpeedController &rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(false)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
	, m_motorRankCount(2)
{
	Startup();
}

/**
 *  Constructs a TankDriveSubsystem with two SpeedControllers.
 */
TankDriveSubsystem::TankDriveSubsystem
(
	SpeedController &leftMotor,
	SpeedController &rightMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesPidEncoderMotors(false)
	, m_drive
	(
		*new RobotDrive
		(
			leftMotor,
			rightMotor
		)
	)
	, m_motorRankCount(1)
{
	Startup();
}

TankDriveSubsystem::~TankDriveSubsystem()
{
}

/**
 *  Gets the number of drive motor ranks (or left motor/right motor pairs) used.
 */
UINT32 TankDriveSubsystem::GetMotorRankCount() const
{
	Action action(this);
	
	return m_motorRankCount;
}

/**
 *  Gets the number of drive motors used.
 */
UINT32 TankDriveSubsystem::GetMotorCount() const
{
	Action action(this);
	
	return m_motorRankCount * 2;
}

/**
 *  Drive the specified amount tank style.
 *  
 *  This function is untested.
 *  
 *  @param leftValue The amount to drive the left drive motors.
 *  @prarm rightValue The amount to drive the right drive motors.
 */
void TankDriveSubsystem::DriveByTank(double leftValue, double rightValue)
{
	Action action(this);
	
	Awake();
	m_drive.TankDrive(leftValue, rightValue, false);
	FeedSafety();
}

/**
 *  Drive the specified magnitude and curve amount.
 *  
 *  This function is untested.
 *  
 *  @param magnitude The magnitude to drive.
 *  @prarm curve the amount to curve.
 */
void TankDriveSubsystem::DriveByMagnitudeAndCurve(double magnitude, double curve)
{
	Action action(this);
	
	Awake();
	m_drive.Drive(magnitude, curve);
	FeedSafety();
}

shared_ptr<Operation<bool> > TankDriveSubsystem::DriveDistanceByTank(TankValue value, double threshold)
{
	Action action(this);
	
	Awake();
	CancelAllOperations();
	return Launch(new DriveDistanceOperation(*this, value, threshold), true);
}

/**
 *  Sets the ControlMode.
 *  
 *  Except for PercentVBus, all other modes will only work if PidEncoderMotorControllers are used.
 *  An exception will be thrown if a mode other than PercentVBus is set without PidEncoderMotorControllers.
 *  The PidEncoderMotorControllers passed to this subsystem may not be able to support every mode usable by this function.
 *  
 *  This function seems to work to the extent that it doesn't crash if used properly.
 *  
 *  @param controlMode The ControlMode to switch to.
 */
void TankDriveSubsystem::SetControlMode(ControlMode controlMode)
{
	Action action(this);
	
	if (m_usesPidEncoderMotors)
	{
		PidMotorController::ControlMode motorControlMode = PidMotorController::ControlMode(controlMode);
		
		{
			MotorIterator it;
			
			for (it = m_motors.begin(); it != m_motors.end(); ++it)
				it->second->SetControlMode(motorControlMode);
		}
		
		if (m_pidValues.count(controlMode) > 0)
		{
			MotorIterator it;
			
			for (it = m_motors.begin(); it != m_motors.end(); ++it)
			{
				PidValue values = m_pidValues[controlMode][it->first];
				
				it->second->SetPid(values.m_p, values.m_i, values.m_d);
			}
			
			//printf("REMOVE THIS AFTER!  PID: %f %f %f\n", leftFront.m_p, leftFront.m_i, leftFront.m_d);
		}
		
		{
			MotorIterator it;
			
			for (it = m_motors.begin(); it != m_motors.end(); ++it)
				it->second->Enable(0);
		}
		
		// Update max output based on control mode.
		switch (controlMode)
		{
			case ControlModePercentVBus:
				SetMaximumOutput(1.0);
				break;
			case ControlModeSpeed:
				SetMaximumOutput(m_maxSpeed);
				break;
			case ControlModeVoltage:
				SetMaximumOutput(m_maxVoltage);
				break;
			case ControlModeCurrent:
				SetMaximumOutput(m_maxCurrent);
				break;
			default:
				SetMaximumOutput(1.0);
				break;
		}
		
		m_controlMode = controlMode;
	}
	else
	{
		if (controlMode == ControlModePercentVBus)
		{
			// No PidEncoderMotorController, but Percent VBus is the normal mode.
			m_controlMode = controlMode;
		}
		else
			throw FpkException("Specified control mode only supported by PidEncoderMotorControllers!");
	}
}

/**
 *  Gets the current ControlMode.
 *  
 *  This function is untested.
 */
DriveSubsystem::ControlMode TankDriveSubsystem::GetControlMode()
{
	Action action(this);
	
	return m_controlMode;
}

/**
 *  Sets the NeutralMode.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 *  
 *  This function is untested.
 *  
 *  @param neutralMode The NeutralMode to switch to.
 */
/*
void TankDriveSubsystem::SetNeutralMode(NeutralMode neutralMode)
{
	Action action(this);
	
	if (m_usesCanJaguars)
	{
		CANJaguar::NeutralMode cjNeutralMode = CANJaguar::NeutralMode(neutralMode);
		
		CanJaguarIterator it;
		
		for (it = m_canJaguars.begin(); it != m_canJaguars.end(); ++it)
			it->second->ConfigNeutralMode(cjNeutralMode);
	}
	else
	{
		throw FpkException("Setting the neutral mode is only supported by CAN Jaguars!");
	}
}
*/

bool TankDriveSubsystem::GetMotorInvertation(TankDriveSubsystem::MotorPosition position) const
{
	Action action(this);
	
	std::map<MotorPosition, bool>::const_iterator it = m_motorInvertations.find(position);
	
	if (it != m_motorInvertations.end())
		return it->second;
	else
		throw FpkException("Unused motor position.");
}

/**
 *  Sets the motor invertations.
 *  
 *  This function is untested.
 *  
 *  @param leftFrontIsInverted The desired invertation of the left front motor.
 *  @param leftBackIsInverted The desired invertation of the left back motor.
 *  @param rightFrontIsInverted The desired invertation of the right front motor.
 *  @param rightBackIsInverted The desired invertation of the right back motor.
 */
void TankDriveSubsystem::SetMotorInvertations
(
	bool leftFrontIsInverted, 
	bool leftBackIsInverted, 
	bool rightFrontIsInverted, 
	bool rightBackIsInverted
)
{
	Action action(this);
	
	SetMotorInvertation(Left, 0, leftFrontIsInverted);
	SetMotorInvertation(Left, 1, leftBackIsInverted);
	SetMotorInvertation(Right, 0, rightFrontIsInverted);
	SetMotorInvertation(Right, 1, rightBackIsInverted);
}

/**
 *  Sets the motor invertations.
 *  
 *  This function is untested.
 *  
 *  @param leftIsInverted The desired invertation of the left motors.
 *  @param rightIsInverted The desired invertation of the right motors.
 */
void TankDriveSubsystem::SetMotorInvertations(bool leftIsInverted, bool rightIsInverted)
{
	Action action(this);
	
	std::map<MotorPosition, bool>::iterator it;
	
	for (it = m_motorInvertations.begin(); it != m_motorInvertations.end(); ++it)
	{
		if (it->first.GetSide() == Left)
		{
			// Left side.
			SetMotorInvertation(it->first, leftIsInverted);
		}
		else
		{
			// Right side.
			SetMotorInvertation(it->first, rightIsInverted);
		}
	}
}

/**
 *  Sets the motor invertation of the motor at the specified MotorPosition.
 *  
 *  This function seems to work.
 *  
 *  @param motorPosition The specified MotorPosition of the motor to be inverted or uninverted.
 *  @param isInverted The desired invertation of the specified motor.
 */
void TankDriveSubsystem::SetMotorInvertation(MotorPosition motorPosition, bool isInverted)
{
	Action action(this);
	
	m_motorInvertations[motorPosition] = isInverted;
	m_drive.SetInvertedMotor(motorPosition.ToWpiValue(GetMotorCount() <= 2), isInverted);
}

void TankDriveSubsystem::SetMotorInvertation(TankDriveSubsystem::Side side, UINT32 rank, bool isInverted)
{
	Action action(this);
	
	SetMotorInvertation(MotorPosition(side, rank), isInverted);
}

/**
 *  Sets the encoder codes per revolution.
 *  
 *  This function is untested.
 *  
 *  @param codePerRevolution The specified encoder codes per revolution of all of the drive motors.
 */
void TankDriveSubsystem::SetEncodersCodesPerRevolution(UINT16 codePerRevolution)
{
	Action action(this);
	
	if (m_usesPidEncoderMotors)
	{
		MotorIterator it;
		
		for (it = m_motors.begin(); it != m_motors.end(); ++it)
			it->second->SetEncodersCodesPerRevolution(codePerRevolution);
	}
	else
		throw FpkException("Using encoders is only supported by PidEncoderMotorController!");
}

/**
 *  Sets the encoder codes per revolution on each of the motors.
 *  
 *  This function is untested.
 *  
 *  @param leftFrontCodesPerRevolution The specified encoder codes per revolution of the left front motor.
 *  @param leftBackCodesPerRevolution The specified encoder codes per revolution of the left back motor.
 *  @param rightFrontCodesPerRevolution The specified encoder codes per revolution of the right front motor.
 *  @param rightBackCodesPerRevolution The specified encoder codes per revolution of the right back motor.
 */
void TankDriveSubsystem::SetEncodersCodesPerRevolution
(
	UINT16 leftFrontCodesPerRevolution, 
	UINT16 leftBackCodesPerRevolution, 
	UINT16 rightFrontCodesPerRevolution, 
	UINT16 rightBackCodesPerRevolution
)
{
	Action action(this);
	
	if (m_usesPidEncoderMotors)
	{
		GetPidMotorController(Left, 0).SetEncodersCodesPerRevolution(leftFrontCodesPerRevolution);
		GetPidMotorController(Left, 1).SetEncodersCodesPerRevolution(leftBackCodesPerRevolution);
		GetPidMotorController(Right, 0).SetEncodersCodesPerRevolution(rightFrontCodesPerRevolution);
		GetPidMotorController(Right, 1).SetEncodersCodesPerRevolution(rightBackCodesPerRevolution);
	}
	else
		throw FpkException("Using encoders is only supported by PidEncoderMotorController!");
}

/**
 *  Sets the encoder codes per revolution on the specified motor.
 *  
 *  This function seems to work.
 *  
 *  @param motorPosition The specified MotorPosition of the motor.
 *  @param codesPerRevolution The specified encoder codes per revolution of the specified motor.
 */
void TankDriveSubsystem::SetEncoderCodesPerRevolution
(
	MotorPosition motorPosition, 
	UINT16 codesPerRevolution
)
{
	Action action(this);
	
	if (m_usesPidEncoderMotors)
		GetPidMotorController(motorPosition).SetEncodersCodesPerRevolution(codesPerRevolution);
	else
		throw FpkException("Using encoders is only supported by PidEncoderMotorControllers!");
}

void TankDriveSubsystem::SetEncoderCodesPerRevolution
(
	Side side,
	UINT32 rank,
	UINT16 codesPerRevolution
)
{
	Action action(this);
	
	SetEncoderCodesPerRevolution(MotorPosition(side, rank), codesPerRevolution);
}

/**
 *  Sets the PID values of each of the motors when in speed control mode.
 *  
 *  This function is only supported when using PidEncoderMotorControllers.
 *  If PidEncoderMotorControllers aren't being used, an exception will be thrown.
 *  
 *  This function is untested.
 *  
 *  @param p The desired Proportional gain.
 *  @param i The desired Intregral gain.
 *  @param d The desired Differential gain. 
 */
void TankDriveSubsystem::SetPIDs
(
	ControlMode controlMode,
	double p, 
	double i, 
	double d
)
{
	Action action(this);
	
	if (m_usesPidEncoderMotors)
	{
		{
			MotorIterator it;
			
			for (it = m_motors.begin(); it != m_motors.end(); ++it)
			{
				m_pidValues[controlMode][it->first] = PidValue(p, i, d);
			}
		}
		
		if (m_controlMode == controlMode)
		{
			MotorIterator it;
			
			for (it = m_motors.begin(); it != m_motors.end(); ++it)
			{
				PidValue value = m_pidValues[controlMode][it->first];
				
				it->second->SetPid(value.m_p, value.m_i, value.m_d);
			}
			
			printf("REMOVE THIS AFTER!  PID: %f %f %f\n", p, i, d);
		}
	}
	else
		throw FpkException("Setting PID values is only supported by PidEncoderMotorControllers!");
}

/**
 *  Sets the PID values of the specified drive motor when in speed control mode.
 *  
 *  This function is only supported when using PidEncoderMotorControllers.
 *  If PidEncoderMotorControllers aren't being used, an exception will be thrown.
 *  
 *  This function seems to work.
 *  
 *  @param motorPosition The specified MotorPosition.
 *  @param p The desired Proportional gain.
 *  @param i The desired Intregral gain.
 *  @param d The desired Differential gain. 
 */
void TankDriveSubsystem::SetPID
(
	ControlMode controlMode,
	MotorPosition motorPosition, 
	double p,
	double i,
	double d
)
{
	Action action(this);
	
	if (m_usesPidEncoderMotors)
	{
		m_pidValues[controlMode][motorPosition] = PidValue(p, i, d);
		
		if (m_controlMode == controlMode)
			GetPidMotorController(motorPosition).SetPid(p, i, d);
	}
	else
		throw FpkException("Setting PID values is only supported by PidEncoderMotorControllers!");
}

void TankDriveSubsystem::SetMaximumSpeed(double maximumSpeed)
{
	Action action(this);
	
	m_maxSpeed = maximumSpeed;
	
	if (GetControlMode() == ControlModeSpeed)
		m_drive.SetMaxOutput(m_maxSpeed);
}

void TankDriveSubsystem::SetMaximumVoltage(double maximumVoltage)
{
	Action action(this);
	
	m_maxVoltage = maximumVoltage;
	
	if (GetControlMode() == ControlModeVoltage)
		m_drive.SetMaxOutput(m_maxVoltage);
}

void TankDriveSubsystem::SetMaximumCurrent(double maximumCurrent)
{
	Action action(this);
	
	m_maxCurrent = maximumCurrent;
	
	if (GetControlMode() == ControlModeCurrent)
		m_drive.SetMaxOutput(m_maxCurrent);
}

double TankDriveSubsystem::GetDistanceThreshold()
{
	Action action(this);
	
	return m_distanceThreshold;
}

void TankDriveSubsystem::SetDistanceThreshold(double distanceThreshold)
{
	Action action(this);
	
	m_distanceThreshold = distanceThreshold;
}

double TankDriveSubsystem::GetExpiration()
{
	Action action(this);
	
	return m_drive.GetExpiration();
}

void TankDriveSubsystem::SetExpiration(double expiration)
{
	Action action(this);
	
	m_safetyHelper->SetExpiration(expiration);
	//m_drive.SetExpiration(expiration);
}

void TankDriveSubsystem::HandlePeriodic()
{
	Action action(this);
	
	if (!m_driveUpdated)
	{
		m_drive.Drive(0, 0);
		m_safetyHelper->Feed();
		
		//printf("REMOVE THIS AFTER!  Drive: Not Updating\n");
	}
	else
		m_driveUpdated = false;
	
	// CANJaguar Fault Checking
	/*
	if (m_usesCanJaguars)
	{
		m_counterPeriods++;
		
		if (m_counterPeriods >= 10)
		{
			CANJaguar &motor = *m_motorCanidateForCheck->second;
			
			if ((m_controlMode == ControlModeSpeed) && (motor.Get() != 0.0) && (motor.GetOutputVoltage() == 0.0))
			{
				motor.EnableControl();
				
				if (motor.GetFaults())
					PrintFaults();
			}
			
			++m_motorCanidateForCheck;
			
			if (m_motorCanidateForCheck == m_canJaguars.end())
				m_motorCanidateForCheck = m_canJaguars.begin();
			
			m_counterPeriods = 0;
		}
	}
	*/
}

void TankDriveSubsystem::HandleAwake()
{
	Action action(this);
	
	if (!GetIsAwake())
	{
		if (m_usesPidEncoderMotors)
		{
			MotorIterator it;
			
			for (it = m_motors.begin(); it != m_motors.end(); ++it)
				it->second->Enable();
		}
	}
}

void TankDriveSubsystem::HandleStop()
{
	m_drive.StopMotor();
	Sleep();
}

/**
 *  Sets the maximum output of the drive system.
 *  
 *  This function seems to work.
 *  
 *  @param maximumOutput The desired maximum output.
 */
void TankDriveSubsystem::SetMaximumOutput(double maximumOutput)
{
	Action action(this);
	
	m_maxOutput = maximumOutput;
	m_drive.SetMaxOutput(maximumOutput);
}

double TankDriveSubsystem::GetMotorInvertationMultiplier(TankDriveSubsystem::MotorPosition position) const
{
	Action action(this);
	
	return GetMotorInvertationMultiplier(GetMotorInvertation(position));
}

double TankDriveSubsystem::GetMotorInvertationMultiplier(TankDriveSubsystem::Side side, UINT32 rank) const
{
	Action action(this);
	
	return GetMotorInvertationMultiplier(MotorPosition(side, rank));
}

PidMotorController &TankDriveSubsystem::GetPidMotorController(TankDriveSubsystem::MotorPosition position)
{
	Action action(this);
	
	if (m_motors.count(position) >= 1)
		return *m_motors[position];
	else
		throw FpkException("Could not find PidEncoderMotorController at specified motor position.");
}

PidMotorController &TankDriveSubsystem::GetPidMotorController(TankDriveSubsystem::Side side, UINT32 rank)
{
	Action action(this);
	
	return GetPidMotorController(MotorPosition(side, rank));
}

void TankDriveSubsystem::FeedSafety()
{
	Action action(this);
	
	m_driveUpdated = true;
	m_safetyHelper->Feed();
}

/**
 *  Prints the faults of the CANJaguars.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, functions will be called on null.
 * 
 *  This function is untested.
 */
/*
void TankDriveSubsystem::PrintFaults(void)
{
	Action action(this);
	
	CanJaguarIterator it;
	
	for (it = m_canJaguars.begin(); it != m_canJaguars.end(); ++it)
		printf("Drive Motors Fault: %d\n", it->second->GetFaults());
}
*/

TankDriveSubsystem::PidValue::PidValue() :
	m_p(0),
	m_i(0),
	m_d(0)
{
}

TankDriveSubsystem::PidValue::PidValue(double p, double i, double d) :
	m_p(p),
	m_i(i),
	m_d(d)
{
}

TankDriveSubsystem::DriveDistanceOperation::DriveDistanceOperation(TankDriveSubsystem &drive, const TankValue &value, double threshold) :
	m_drive(drive),
	m_value(value),
	m_threshold(threshold)
{
}

void TankDriveSubsystem::DriveDistanceOperation::HandleLaunch()
{
	m_controlModePrevious = m_drive.GetControlMode();
	m_drive.SetControlMode(ControlModeDistance);
}

void TankDriveSubsystem::DriveDistanceOperation::HandlePeriodic()
{
	bool finished = true;
	UINT32 rank;
	
	for (rank = 0; rank < m_drive.GetMotorRankCount(); ++rank)
	{
		PidMotorController &motorLeft = m_drive.GetPidMotorController(TankDriveSubsystem::Left, rank);
		PidMotorController &motorRight = m_drive.GetPidMotorController(TankDriveSubsystem::Right, rank);
		
		double positionLeftMotorTarget = m_value.GetLeftValue() * m_drive.GetMotorInvertationMultiplier(Left, rank);
		double positionRightMotorTarget = m_value.GetRightValue() * m_drive.GetMotorInvertationMultiplier(Right, rank);
		
		motorLeft.Set(positionLeftMotorTarget);
		motorRight.Set(positionRightMotorTarget);
		
		double positionLeftCurrent = motorLeft.GetPosition() * m_drive.GetMotorInvertationMultiplier(Left, rank);
		double positionRightCurrent = motorRight.GetPosition() * m_drive.GetMotorInvertationMultiplier(Right, rank);
		
		finished = finished && (abs(positionLeftCurrent - m_value.GetLeftValue()) < m_threshold);
		finished = finished && (abs(positionRightCurrent - m_value.GetRightValue()) < m_threshold);
	}
	
	m_drive.FeedSafety();
	
	if (finished)
	{
		m_drive.SetControlMode(m_controlModePrevious);
		Finish(true);
	}
}

bool TankDriveSubsystem::DriveDistanceOperation::HandleCancel()
{
	m_drive.SetControlMode(m_controlModePrevious);
	return false;
}

void TankDriveSubsystem::SetPidMotorController(TankDriveSubsystem::MotorPosition position, shared_ptr<PidMotorController> motor)
{
	Action action(this);
	
	m_motors[position] = motor;
}

void TankDriveSubsystem::SetPidMotorController(TankDriveSubsystem::Side side, UINT32 rank, shared_ptr<PidMotorController> motor)
{
	Action action(this);
	
	SetPidMotorController(MotorPosition(side, rank), motor);
}

void TankDriveSubsystem::SetPidMotorController(TankDriveSubsystem::Side side, UINT32 rank, PidMotorController *motor, bool canDeleteMotor)
{
	Action action(this);
	
	if (canDeleteMotor)
		SetPidMotorController(MotorPosition(side, rank), shared_ptr<PidMotorController>(motor));
	else
		SetPidMotorController(MotorPosition(side, rank), shared_ptr<PidMotorController>(motor, NullDeleter()));
}

void TankDriveSubsystem::SetPidMotorController(TankDriveSubsystem::Side side, UINT32 rank, PidMotorController &motor)
{
	Action action(this);
	
	SetPidMotorController(MotorPosition(side, rank), shared_ptr<PidMotorController>(&motor, NullDeleter()));
}

void TankDriveSubsystem::SetPidMotorController(TankDriveSubsystem::Side side, UINT32 rank, CANJaguar *motor, bool canDeleteMotor)
{
	Action action(this);
	
	SetPidMotorController(side, rank, new CanJaguarWrapper(motor, canDeleteMotor), true);
}

void TankDriveSubsystem::SetPidMotorController(TankDriveSubsystem::Side side, UINT32 rank, CANJaguar &motor)
{
	Action action(this);
	
	SetPidMotorController(side, rank, new CanJaguarWrapper(&motor, false), true);
}

void TankDriveSubsystem::Startup()
{
	UINT32 motorPairs = m_motorRankCount;
	UINT32 rank;
	
	for (rank = 0; rank < motorPairs; ++rank)
	{
		SetMotorInvertation(Left, rank, false);
		SetMotorInvertation(Right, rank, true);
	}
	
	m_driveUpdated = false;
	m_maxOutput = 1.0;
	m_distanceThreshold = 0.5;
	m_counterPeriods = 0;
	
	m_controlMode = ControlModePercentVBus;
	
	m_drive.SetMaxOutput(m_maxOutput);
	
	SetControlMode(m_controlMode);
	
	if (m_usesPidEncoderMotors)
		m_motorCanidateForCheck = m_motors.begin();
	
	/*
	 *  Syncing this safety helper with the periodic timing is a pain.
	 *  If the periodic loop is getting stuck, watchdog should timeout anyway.
	 */
	m_safetyHelper = new MotorSafetyHelper(&m_drive);
	//m_safetyHelper->SetSafetyEnabled(true);
	m_safetyHelper->SetSafetyEnabled(false);
	m_drive.SetSafetyEnabled(false);
}

double TankDriveSubsystem::GetMotorInvertationMultiplier(bool isInverted)
{
	if (isInverted)
		return -1;
	else
		return 1;
}
